package org.pi.common.vision.tool

import org.pi.common.matrix.FloatMatrix

import org.pi.common.vision.tool.ConvolutionTool._

trait GradientToolTrait {
	private var tempMatrix : FloatMatrix = null

	def gradient(I: FloatMatrix) : (FloatMatrix, FloatMatrix) = {
		val t = gradient(I, null, null, tempMatrix)
		tempMatrix  = t._3
		(t._1, t._2)
	}

	def gradient(I: FloatMatrix, IxResultParam: FloatMatrix, IyResultParam: FloatMatrix) : (FloatMatrix, FloatMatrix) = {
		val t = gradient(I, IxResultParam, IyResultParam, tempMatrix)
		tempMatrix  = t._3
		(t._1, t._2)
	}

	def gradient(I: FloatMatrix, IxResultParam: FloatMatrix, IyResultParam: FloatMatrix, tempParam:  FloatMatrix) : (FloatMatrix, FloatMatrix, FloatMatrix) = {
		var IxResult = IxResultParam
		var IyResult = IyResultParam
		var temp = tempParam
		//if ((null == IxResult) || !IxResult.hasSameShape(I)) IxResult = new FloatMatrix( I.shape() :_*)
		//if ((null == IyResult) || !IyResult.hasSameShape(I)) IyResult = new FloatMatrix( I.shape() :_*)
		//if ((null == temp) || !temp.hasSameShape(I)) temp = new FloatMatrix( I.shape() :_*)
		if ((null == IxResult) || !IxResult.hasSameShape(I)) IxResult = new FloatMatrix( I.shape() :_* )
		if ((null == IyResult) || !IyResult.hasSameShape(I)) IyResult = new FloatMatrix( I.shape() :_* )
		if ((null == temp) || !temp.hasSameShape(I)) temp = new FloatMatrix( I.shape() :_*)
		val diffFilter = FloatMatrix( (-1.0f) :: 0.0f :: 1.0f :: Nil )
		val binoFilter = FloatMatrix( 0.25f :: 0.5f :: 0.25f :: Nil )
		filter2Seperated(I,diffFilter,temp,0,BorderMode.Replicate, NormalizeMode.NoNormalize)
		filter2Seperated(temp,binoFilter,IyResult,1,BorderMode.Replicate, NormalizeMode.NoNormalize)
		filter2Seperated(I,binoFilter,temp,0,BorderMode.Replicate, NormalizeMode.NoNormalize)
		filter2Seperated(temp,diffFilter,IxResult,1,BorderMode.Replicate, NormalizeMode.NoNormalize)
		(IxResult, IyResult, temp)
	}

	def abs(x: FloatMatrix, y:FloatMatrix) : FloatMatrix = abs(x,y,null)
	
	def abs(x: FloatMatrix, y:FloatMatrix, resultParam:FloatMatrix) : FloatMatrix = {
		var result = resultParam
		if (!y.hasSameShape(x)) throw new Error("matrices must have same size.")
		//if ((null == result) || !result.hasSameShape(x)) result = new FloatMatrix( x.shape() :_*)
		if ((null == result) || !result.hasSameShape(x)) result = new FloatMatrix( x.shape() :_*)
		for ( index <- 0 until result.length) {
			result(index) = Math.sqrt( (x(index)*x(index) + y(index)*y(index)).toDouble ).toFloat
		}
		result
	}

	def angle(x: FloatMatrix, y:FloatMatrix) : FloatMatrix = angle(x,y, null)

	def angle(x: FloatMatrix, y:FloatMatrix, resultParam:FloatMatrix) : FloatMatrix = {
		var result = resultParam
		if (!y.hasSameShape(x)) throw new Error("matrices must have same size.")
		//if ((null == result) || !result.hasSameShape(x)) result = new FloatMatrix( x.shape() :_*)
		if ((null == result) || !result.hasSameShape(x)) result = new FloatMatrix( x.shape() :_*)
		for ( index <- 0 until result.length) {
			result(index) = Math.atan2( y(index).toDouble, x(index).toDouble ).toFloat
		}
		result
	}
}

object GradientTool extends GradientToolTrait {
	
	def main(args: Array[String]) : Unit =  {
		println("main grad start")
		import org.pi.common.vision.Api.imread
		import org.pi.common.vision.Api.imwrite
		import org.pi.common.vision.Api.rgb2gray
		import org.pi.common.time.Api._
		
		val c = imread("data/4Spiele.png")
		val g = rgb2gray(c)
		
		tic
		val (fgx, fgy) = gradient(g)
		toc("gradient computation")

		tic
		val fg = abs(fgx,fgy)
		val fa = angle(fgx,fgy)
		toc("gradient post proc.")
		
		imwrite("gray.png",g)
		imwrite("gray_filtered_y.png",fgy)
		imwrite("gray_filtered_x.png",fgx)
		imwrite("gray_absgrad.png",-fg)
		imwrite("gray_arggrad.png",fa)
		
		println("main conv end")
	}
}

